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DESCRIPTION 
FLOATING MOBILE OBJECT CONTROL SYSTEM 



TECHNICAL FIELD 

5 [0001] The present invention relates to control systems for 
floating mobile objects in water, air, outer space, etc., or mobile 
objects for sliding on a plane. 



BACKGROUND ART 

10 [0002] First, a prior known floating mobile object' control 
technique is described with reference to FIGS. 11 through 14 and 
so on. FIG. 11 is a schematic diagram of a prior Icnown floating 
mobile object, and FIGS. 11 through 14 are schematic diagrams 
illustrating a prior Icnown floating mobile object control 

15 technique. 

[0003] As shown in FIG. 11, in order to control the position and 
velocity of the main body of a floating mobile object 10 such as 
an underwater robot, the main body is provided with a sensor 13 
by which the position and velocity of the main body of the floating 
20 mobile object 10 are sensed and fed to a subtraction circuit 11 
in a computer P. The subtraction circuit 11 is also fed with a 
signal indicating desired position and velocity from an operator 
H . An output of the subtraction circuit 11 is provided to a thruster 
control circuit 12 for controlling thrusters T provided as thrust 
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generation means (effectors) on the floating mobile object 10, 
thereby controlling the position and velocity of the floating 
mobile object 10 (which is what is called position/velocity 
feedback control) . 
5 [0004] FIG, 12 is a block diagram for controlling the floating 
mobile object 10 shown in FIG. 11. An effector thrust command 
signal, which is an output of the thruster control circuit 12, 
is fed to the thrusters T having a predetermined thrust 
characteristic 14, thereby feeding an effector thrust to the 

10 floating mobile object 10, so that dynamics of the floating mobile 
object 10, i.e., dynamic input/output characteristics, are 
obtained as denoted by reference character 15, and the position 
and- velocity of the main body is sensed by the sensor 13 and fed 
to the subtraction circuit 11 as described above. 

15 [0005] As such, in the conventional control of a floating mobile 
object, information concerning the position and velocity of a 
floating mobile object, which is a subject of control, is fed back 
to directly handle a thrust command to an effector, and in the 
case of a floating mobile object generally having an effector with 

20 a relatively slow response speed, it takes a time while after the 
command is sent to the effector, the effect of which is reflected 
in the position and velocity of the floating mobile object, until 
the sensor obtains them, therefore there arises a problem where 
control performance is adversely affected (which is what is called 
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a sensing delay problem) . This tendency becomes noticeable as 
the mass of the floating mobile object is increased or the thrust 
of the effector is decreased. 

Further, a floating mobile object in a stream of water or 

• 5 air is always affected by hydrodynamic disturbances such as waves 
and tidal current or wind, and in addition, as shown in FIG. 11, 
the f loatingmobile object 10 in water is connected with an umbilical 
cable C (a feeder, a communication line or the like) from above 
the water, which often pulls the floating mobile object 10, 

10 therefore in the case of the above-described control technique, 
a response to unknown disturbances takes time, causing a problem 
where control performance is adversely affected. 
[0006] Next, in the case where the floating mobile object 10 is 
further provided with a robot arm A having a plurality of links 

15 as shown in FIG. 11, a block diagram as illustrated in FIG. 13 
is constructed. A reaction force and torque by the robot arm A 
as denoted by reference character 16 is added to effector thrusts 
by the thrusters T as illustrated by an adding circuit 17. 
Accordingly, the reaction force and torque from the robot arm A 

20 disturbs the action of the floating mobile object 10, i.e., the 
position and velocity, and the thrusters T are controlled to correct 
errors in the position and velocity of the floating mobile object 
10 that are caused by the reaction force and torque of the robot 
arm A. As such, the action (position and velocity) of the floating 
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mobile object 10 is disturbed by the reaction force and torque 
from the robot arm A, and therefore conventional attitude control 
activates the thrusters T so as to correct the resulting errors 
in the position and velocity. 
5 [0007] Specifically, the prior known control technique operates 
activates the thrusters T after the floating mobile object 10 is 
moved by the reaction force and torque of the robot arm A, and 
therefore the so-called sensing delay problem where a response 
speed to disturbances is low still remains unsolved. 

10 [0008] FIG. 14 illustrates a block diagram of another conventional 
art. This control technique takes into consideration mutual 
interference between the motion of the robot arm A and the motion 
of themainbody of the floatingmobile object 10, and simultaneously 
determines an effector thrust and a robot arm joint torque, such 

15 that their motions are consistent with their respective purposes. 
In this conventional art, the floating mobile object 10 provided 
with the robot arm A is configured as a multi-link system, and 
based on its equation of motion, influence of each joint torque 
of each robot arm on the motion of the main body of the floating 

20 mobile object 10 is expressed in a mathematical formula, which 
is solved to determine propulsion power of the main body of the 
floating mobile object 10 (which is what is called model-based 
control) . 

[0009] In the conventional art of FIG. 14, an effector thrust 
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command signal from a thruster/robot arm control circuit 18 is 
fed to the thrusters T, and a torque command signal of the robot 
arm A is fed to a joint actuator 19 . As such, complex system dynamics 
20 of the main body of the floating mobile object 10 and the robot 
5 arm A are formed by effector thrusts outputted from the thrusters 
T and a joint torque by the joint actuator 19, and the sensor 13 
senses the position and velocity of the main body of the floating 
mobile object 10 and feeds them to the subtracter 11 to which a 
signal representing desired position and attitude of the main body 

10 of the floating mobile object 10 is fed. Also, robot arm control 
variables from the complex system dynamics 20, i.e., signals 
representing the position, attitude, joint angle, velocity, hand 
reaction force, etc., are fed to the subtraction circuit 21 to 
which target values for the control variables of the robot arm 

15 A are fed. Outputs of the subtracters 11 and 21 are fed to the 
thruster/robot arm control circuit 18. 

Specifically, this control technique takes into 
consideration the mutual interference between the motion of the 
robot arm A and the motion of the main body of the floating mobile 

20 object 10, and simultaneously determines an effector thrust and 
a robot arm joint torque, such that their motions are consistent 
with their respective purposes . While this advantageously allows 
the motion of the main body of the floating mobile object 10 to 
be accurately controlled, there are the following problems (1) 



SUBSTITUTE SPECIFICATION 
F-9163 Inventor: Katsuya KANAOKA 

through (3) . 

[0010] (1) The object dynamics are generally complicated, and if 
the robot arm A has six axes, the dynamics take a form of a link 
joining seven rigid bodies including the main body of the floating 
5 mobile object 10, In the case where there are a plurality of robot 
arms A, the dynamics become more significantly complicated. 
Arithmetic of such control system is considerably time-consuming, 
and imposes a heavy computation load on the computer. Therefore, 
it is practically very difficult to configure the above-described 

10 control system with a small computer. 

[0011] (2) Also, the above-described control system requires a 
number of dynamic parameters such as the mass, moment of inertia 
and gravitational center position of each link of the robot arm 
A and the main body, and proper control cannot be realized by using 

15 these dynamic parameters unless their values are all correct. 
Accordingly, in the case of holding an object with the robot arm 
A, the mass, moment of inertia and gravitational center position 
of the held object should be provided appropriately. Thus, it 
is required to have a database of ob j ects that can be held or measure 

20 dynamic characteristics of the object, therefore there arises a 
problem where the use is practically limited, 

(3) Further, a floating mobile object in a stream of water 
or air is always affected by unknown hydrodynamic disturbances 
such as waves and tidal current or wind, and in addition, as shown 
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in FIG. 11^ the floating mobile object 10 in water is connected 
with an umbilical cable C (a feeder, a communication line or the 
like) from above the water, which often pulls the floating mobile 
object 10, besides it is not possible to model the unknown 
5 disturbances, and in addition it is difficult to predict influence 
of the disturbances , therefore such a model-based control technique 
is disadvantageous in that it cannot be fundamentally adapted to 
control of a floating mobile object. 

[0012] Other prior known floating mobile object control techniques 
10 include, for example, feedback control of force at a joint of a 
manipulator as disclosed in Patent Document 1 and local feedback 
control of a thrust as disclosed in Non-patent Document 1. 
[0013] According to the feedback control of a force at a joint 
of a manipulator in Patent Document 1, it is possible to activate 
15 thrusters before the main body of a floating mobile object is moved 
due to disturbances by the manipulator, thereby canceling any 
influence on the main body of the floating mobile object and 
preventing the main body of the floating mobile object from being 
moved by the disturbances by the manipulator. 
20 [0014] Also, the thrusters used as effectors for controlling the 
position/velocity of the floating mobile object generates a thrust 
by taking advantage of a stream, and therefore a high nonlinearity 
caused by the stream is present between an input command to the 
thrusters and a thrust that is actually outputted. Such a 

7 
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nonlinearity results from various factors, and conventionally it 
is difficult to perform control with consideration of all of them 
(a problem of what is called a nonlinearity of an effector dynamic 
characteristic) . However, according to the local feedback 
5 control of a thrust in Non-Patent Document 1, it is possible to 
prevent control performance for the floating mobile object from 
deteriorating due to the nonlinearity of effector dynamic 
characteristic. 

[0015] While both of the above-described control techniques 
10 according to Patent Document 1 and Non-Patent Document 1 measure 
and feed back force partially, they do not cancel influence of 
waves and tidal current or wind which are the greatest disturbances 
to the floating mobile object. 

[0016] In brief, neither of the above-described conventional arts 
15 realizes a floating mobile object control system capable of 
automatically compensating for disturbing influence even under 
disturbances caused by reaction, etc . , of a robot arm and influence 
of unknown hydrodynamic disturbances such as waves and tidal 
current or wind to which a floating mobile object in water, air 
20 or the like is always subjected, thereby allowing the floating 
mobile object to stand still with high precision or track a target 
trajectory with high precision. 

[0017] On the other hand, aside from the above-described problems, 
there is a below-described problem with inertial navigation, which 

8 
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is a prior known technique widely used for navigating a floating 
mobile object. Specifically, the inertial navigation uses an 
acceleration obtained from a floating mobile object, and in the 
case where the floating mobile object itself is large and heavy, 
5 a prior known acceleration sensor, which temporally converts an 
acceleration to a displacement and outputs the value of 
acceleration as an electric signal, is structurally unable to sense 
a minor change in the acceleration with high precision, therefore 
there arises a problem where a noticeable error is present between 

10 the acceleration value obtained from the sensor and an actual 
acceleration value . Such an error between the acceleration values 
considerably affects the accuracy of the inertial navigation itself, 
causing inconvenience of being unable to accurately guide the 
floating mobile object onto a desired trajectory. 

15 [Patent Document 1] Japanese Laid-Open Patent Publication 

No. 5-119837 

[Non-Patent Document 1] Kanaoka, Nakayama, Hayashi and 
Kawamura, "Thrust Local Feed-Back Control for Realization of High 
Speed and Precise Motion in Water", proceedings of the Robotics 
20 and Mechatronics Conference '03, the Japan Society of Mechanical 
Engineers, 2P1-2F-A6, 2003 

DISCLOSURE OF THE INVENTION 
PROBLEMS TO BE SOLVED BY THE INVENTION 

9 
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[0018] Therefore, an objective of the present invention is to 
provide a floating mobile object control system capable of causing 
a floating mobile object to stand still in a predetermined position 
with high precision or track a target tra j ectory with high precision, 
5 even under disturbances caused by, for example, waves and tidal 
current or wind or disturbances caused by reaction, etc., of a 
robot arm attached to the floating mobile object. 
[0019] In addition, an objective of the present invention is to 
provide a floating mobile object control system having a relatively 
10 simple structure and capable of accurately controlling the position, 
attitude, etc., of a floating mobile object. 

[0020] Further, an objective of the present invention is to provide 
a floating mobile object acceleration sensing system capable of 
sensing an acceleration of a floating mobile object with high 
15 precision to such an extent that a minor change in acceleration 
can be sensed, regardless of size and weight of the floating mobile 
object or without depending on the capability of an acceleration 
sensor. 

20 SOLUTION TO THE PROBLEMS 

[0021] The inventor of the present invention completed the 
invention based on findings that by dividing a floating mobile 
object into a main body part that can be considered as a single 
rigid body constituting a part of the floating mobile object and 

10 
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an effector part for generating a thrust for the floating mobile 
object, and providing a force/torque sensor at an only portion 
(a thrust transfer gate) that dynamically connects the main body 
part and the effector part, it is made possible to measure all 
5 forces and torques applied between the main body part and the 
effector part, because a value sensed by the force/torque sensor 
is a thrust fed from the effector part to the main body part, and 
directly control a thrust that is to be fed to the floating mobile 
object by feeding back the value, whereby it is possible to allow 
10 the floating mobile object to stand still in a predetermined 
position with high precision, or track a target trajectory with 
high precision, even under hydrodynamic disturbances caused by 
waves, tidal current, etc. 

[0022] A control system for a floating mobile object of the present 
15 invention that can solve the above objectives is (1) a control 
system for a floating mobile object, in which the floating mobile 
object includes : a main body part that can be considered as a single 
rigid body constituting a part of the floating mobile object; an 
effector part for generating a thrust for the floating mobile 
20 object; and a thrust transfer gate for dynamically connecting the 
main body part and the effector part, the thrust transfer gate 
being adapted to be able to actually measure a thrust from the 
effector part acting on the main body part, the main body part 
includes acceleration measurement means capable of measuring an 

11 
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acceleration of the floating mobile object, and a thrust command 
to the effector part is obtained by using: an output from the 
acceleration measurement means of the main body part ; and a measured 
thrust value from the thrust transfer gate. 
5 [0023] Further, in the control system for a floating mobile object 
according to the present invention, (3) the thrust transfer gate 
includes force/torque measurement means capable of measuring a 
force/torque applied between the main body part and the effector 
part, and the thrust command to the effector part is obtained by 

10 using: an output from the force/torque measurement means of the 
thrust transfer gate; the output from the acceleration measurement 
means of the floating mobile object; and a target acceleration 
trajectory command obtained by converting an externally inputted 
target position/velocity trajectory command. 

15 [0024] Note that in the case of applying a control system and an 
acceleration sensing system of the present invention to a floating 
mobile object for sliding on a slope as well as a floating mobile 
object for floating in three-dimensional space, which will be 
described in each embodiment and example below, it is necessary 

20 to compensate for influence of a gravitational acceleration, 
therefore it is required to provide inclination angle measurement 
means capable of measuring an inclination of the main body part 
as defined by the invention according to (7) below. 

However, when applying the control system or acceleration 

12 
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sensing system of the present invention to a floating mobile object 
for floating in outer space where basically no gravity is applied 
or a floating mobile object for sliding on a horizontal plane for 
which it is not necessary to consider any inclination or it is 
5 possible to ignore any inclination, the inclination angle 
measurement means as defined by the invention according to the 
above (1) or (3), or the next-described (2) or (4) is not required. 

The same can be said of the invention according to (5) or 
(6) below to which the notion of a virtual thrust transfer gate 

10 system is applied, and in the case of applying a control system 
of the present invention, which employs the notion of the virtual 
thrust transfer gate system, to a floating mobile object for 
floating in three-dimensional space or the like, it is required 
toprovide inclination anglemeasurementmeans capable of measuring 

15 an inclination of the floating mobile object as defined by the 
invention according to (6) below. 

[0025] Similarly, a control system for a floating mobile object 
of the present invention that can solve the above objectives is 
(2) a control system for a floating mobile object, in which the 
20 floating mobile object includes: a main body part that can be 
considered as a single rigidbody constituting apart of the floating 
mobile object; an effector part for generating a thrust for the 
floating mobile object; and a thrust transfer gate for dynamically 
connecting the main body part and the effector part, the thrust 

13 
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transfer gate being adapted to be able to actually measure a thrust 
from the effector part acting on the main body part, the main body 
part is dynamically connected only to the thrust transfer gate, 
the main body part being adapted to receive substantially all forces 
5 acting thereon via the thrust transfer gate, and a thrust command 
to the effector part is obtained by using a measured thrust value 
from the thrust transfer gate. 

Note that in a typical example where the main body part 
is dynamically connected only to the thrust transfer gate and 

10 adapted to receive substantially all forces acting thereon via 
the effector part and the thrust transfer gate, the effector part 
is in the form of a shell enclosing the main body part. 

However, such a configuration is an extreme example, and 
the main body part is not necessarily sealed in an effector part 

15 having the form of a shell. In other words, it is only required 
to structure the invention defined in (2) such that the main body 
part is not directly subjected to disturbances such as water 
pressure or intense wind pressure or other external forces, and 
if the main body part contained in a floating mobile object for 

20 sliding on water or ground, for example, is exposed to air, it 
is merely an ignorable problem that does not cause any particular 
inconvenience in application of the invention according to (2) . 

Thus, it is understood that the invention defined in (2) 
is not limited to the configuration as shown in FIG. 3. 

14 
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[0026] Further, in the control system for a floating mobile object 
according to the present invention as described in (2) above, (4) 
the thrust transfer gate includes force/torque measurement means 
capable of measuring a force/torque applied between the main body 
5 part and the effector part, and the thrust command to the effector 
part is obtained by using: an output from the force/torque 
measurement means of the thrust transfer gate; and a target 
acceleration trajectory command obtained by converting an 
externally inputted target position/velocity trajectory command. 

10 [0027] Further, the present invention is directed to (5) a control 
system for a floating mobile object, in which the floating mobile 
object includes acceleration measurement means capable of an 
acceleration of the floating mobile object, the floating mobile 
object is arithmetically divided into: a virtual main body part 

15 that can be considered as a single rigid body constituting a part 
of the floating mobile object; a virtual effector part for 
generation a thrust for the floating mobile object; and a virtual 
thrust transfer gate for virtually connecting the virtual main 
body part and the virtual effector part, the virtual thrust transfer 

20 gate being adapted to be able to estimate a thrust from the virtual 
effector part acting on the virtual main body part based on an 
output from the acceleration measurement means, and a thrust 
command to the virtual effector part is obtained by using an estimate 
thrust value from the virtual thrust transfer gate. 
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In addition^ in the control system for a floating mobile 
object according to the present invention as described in (5) above, 
(6) the virtual thrust transfer gate includes force/torque 
estimation means capable of estimating a force/torque applied 
5 between the virtual main body part and the virtual effector part, 
and the thrust command to the virtual effector part is obtained 
by using: an output from the force/torque estimation means of the 
virtual thrust transfer gate; and a target acceleration trajectory 
command obtained by converting an externally inputted target 

10 position/velocity trajectory command. 

In addition, in a control system based on the control system 
for a floating mobile object as described in (1) through (6) above, 
which is configured to be able to eliminate gravitational influence 
as described above, (7) the main body part or the virtual main 

15 body part additionally includes inclination angle measurement 
means capable of measuring an inclination of the floating mobile 
object, and the thrust command to the effector part or the virtual 
effector part is obtained by additionally using an output from 
the inclination angle measurement means. 

20 In addition, the present invention is directed to a control 

system for a floating mobile object intended to offer higher 
performance by additionally provided with a thrust transfer gate 
system according to (1) through (7) above and a prior known 
position/velocity control capability, in which (8) the floating 

16 
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mobile object additionally includes position/velocity 
measurement means capable of measuring a position or a velocity 
of the floating mobile object, and the thrust command to the effector 
part or the virtual effector part is obtained by additionally using 
5 an output from the position/velocity measurement means and an 
externally inputted target position command or target velocity 
command . 

[0028] Note that as described below, in the above control system 
for a floating mobile object according to the present invention, 

10 control of the floating mobile object is carried out in two stages : 
a "thrust planning part", which issues a command after calculating 
a thrust that should be applied from the thrust transfer gate to 
the main body part in order to realize a target trajectory, and 
a "thrust control part", which directly drives the effector part, 

15 so as to realize a command thrust at the thrust transfer gate, 
by feeding back the current thrust value at the thrust transfer 
gate to the effector part. 

[0029] In the present invention, the above mechanism design and 
control technique are collectively referred to as the "thrust 
20 transfer gate system" . The thrust transfer gate system is mainly 
embodied in the following three forms: 

i) the main body part is exposed to the exterior, i.e., 
the main body part is directly affected by disturbances such as 
water pressure or intense wind pressure or other external forces, 

17 
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and a thrust transfer gate is further provided [see the 
below-described first instance of a first embodiment] ; 

ii) a thrust transfer gate is provided, and in order not 
to subject the main body part directly to disturbances such as 

5 water pressure or intense wind pressure or other external forces, 
the main body part is dynamically connected only to the thrust 
transfer gate and adapted to receive substantially all forces 
acting thereon via the effector part and the thrust transfer gate 
(e.g., the main body part is covered and sealed in the effector 
10 part) [see the below-described second instance of the first 
embodiment]; and 

iii) the floating mobile object is aritlimetically divided 
into the main body part and the effector part without changing 
any hardware component of the f loatingmobile object, and an inertia 

15 matrix for the main body part is previously prepared in a computer, 
and used together with an output of an acceleration sensor provided 
in the floating mobile object and, if necessary, an output from 
an inclination angle sensor provided in the floating mobile object, 
to obtain a thrust command to the effector part [see the 

20 below-described third instance of the first embodiment for a 
virtual thrust transfer gate] . 

[0030] Ini) andii), achieved control performance is high, although 
it is required to change the hardware design of the floating mobile 
object due to the thrust transfer gate system, and the force/torque 

18 
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sensor is required at the thrust transfer gate. 

In iii), on the other hand, control performance is low 
compared to the methods in i) and ii), but it is not required to 
change the hardware design of the floating mobile object, and by 
5 adding the acceleration sensor and, if necessary, the inclination 
angle sensor to an existing floating mobile object, it is made 
possible to utilize the thrust transfer gate system, thereby 
improving the control performance. 

Particularly, a known inertial navigation device, which 

10 is currently mounted in aircrafts, etc., can also be used as the 
acceleration sensor, inclination angle sensor or main body part 
position/velocity sensor used in the present invention, and by 
using this, it is made possible to realize in an increase in speed 
and precision compared to existing control, without making any 

15 hardware modification. 

[0031] In addition, the present invention is directed to a floating 
mobile object acceleration sensing system for sensing an 
acceleration of a floating mobile object, in which the floating 
mobile object includes: a main body part that can be considered 

20 as a single rigid body constituting a part of the floating mobile 
object; an effector part for generating a thrust for the floating 
mobile object; and a thrust transfer gate for dynamically 
connecting the main body part and the effector part, the thrust 
transfer gate being adapted to be able to actually measure a thrust 

19 
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from the effector part acting on the main body part, the thrust 
transfer gate includes force/torque measurement means capable of 
measuring a force/torque applied between the main body part and 
the effector part, the main body part is dynamically connected 
5 only to the thrust transfer gate, the main body part being adapted 
to receive substantially all forces acting thereon via the effector 
part and the thrust transfer gate, and an estimated acceleration 
value for the main body part is obtained by using an output from 
the force/torque measurement means from the thrust transfer gate. 

10 Also, in the floating mobile object acceleration sensing 

system according to the present invention, the main body part 
includes inclination angle measurement means capable of measuring 
an inclination of the main body part, and the estimated acceleration 
value for the main body part is obtained by additionally using 

15 an output from the inclination angle measurement means of the main 
body part. 

[0032] According to the present invention, it is possible to provide 
a system structure capable of allowing a floating mobile object 
to stand still in a predetermined position with high precision, 
20 or track a target trajectory with high precision, even under 
disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
to the f loatingmobile object, thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 

20 
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[0033] Note that terms used for describing the present invention 
are defined below. 

[0034] The term "floating mobile object" is meant to indicate 
various mobile objects for floating in water, air, outer space, 
5 and so on. Also, mobile objects for sliding on a plane or rails 
fall within the notion of the floating mobile object. 

Concrete examples falling within the notion of the "floating 
mobile object" as defined by the present invention are listed below 
by environmental type: 
10 (1) outer space: space robots, spaceships, artificial 

satellites, etc. ; 

(2) air: aerial robots, aircrafts, helicopters, airships, 

etc . ; 

(3) underwater: underwater robots, submersibles, etc.; 
15 (4) overwater: watercrafts, air-cushion vehicles, etc.; 

(5) ground: cars, trains, air-cushion vehicles, other 
mobile objects for sliding on ice or snow, such as sleds and 
snowmobiles, etc. 

As for ground vehicles such as cars, it is understood that 
20 they can be the "floating mobile object" when their wheels are 
sliding, for example. Accordingly, by applying the present 
invention to, for example, a car, it is possible to control the 
vehicle similarly regardless of whether its wheels are sliding 
or not. 
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However, when the wheels are sliding, thrust transfer means 
composed of existing engine and tires cannot function. In view 
of this, it is understood from each of the following descriptions 
that when applying the present invention under the above 
5 circumstance, it is preferred that the ground vehicle is provided 
with any other thrust transfer means (various types of thrusters 
or the like) . 

[0035] The term "main body part" is meant to indicate a physical 
structure that can be considered as a single rigid body constituting 

10 a part of the floating mobile object. In principle, the main body 
part does not include any mechanically moving part. Even when 
any mechanically moving part is included, no problem is caused 
if its influence is as small as it can be ignored, and when it 
cannot be ignored, still it is possible to cancel the influence 

15 if it is predictable. 

Note that in the invention that corresponds to the 
below-described third instance of the first embodiment according 
to the present invention to which the notion of the virtual thrust 
transfer gate system is applied, the rigid distinction between 

20 the main body part and the effector part is merely for convenience, 
users may be allowed to use their judgment to determine which part 
shouldbe defined as the main body part . Accordingly, when actually 
carrying out the invention that corresponds to the below-described 
third instance of the first embodiment, the main body part may 

22 
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be defined as constituting the most part of the mass of the floating 
mobile object, or on the contrary, the main body part may be defined 
as an object additionally provided in a part of the floating mobile 
object (e.g., a weight of about several kilograms). 

Also, in the inventions that correspond to the 
below-described first and second instances of the first embodiment 
to which the notion of the virtual thrust transfer gate system 
is not applied, whether or not the main body part and the effector 
part are connected by the below-described thrust transfer gate 
is the only judgment criterion for rigidly distinguishing between 
them, and there is no limitation on proportion of the main body 
part in the floating mobile object so long as such criterion is 
satisfied. 

Thus, in the present invention, the main body part is not 
limited to a physical structure composed of a single rigid body 
constituting the most part of the mass of the floating mobile object . 
[0036] The term "effector part" is meant to indicate a physical 
structure for generating a thrust for the floating mobile object. 
All thrust generation means (effectors) for controlling the 
position, velocity, etc., of the floating mobile object are 
included. 

[0037] The term "dynamically connected" is meant to indicate a 
state where some Icind of interaction of force is taking place between 
objects that are referred to thereby. In most cases, they are 
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mechanically connected, but cases where, even when not mechanically 
connected, non-contact interaction of force is applied by, for 
example, electromagnetic force, are also included. 

On the other hand, the term "mechanically divided" is meant 
to indicate a case where no interaction of force is applied between 
objects that are referred to thereby. 

[0038] The term "thrust transfer gate" is meant to indicate the 
only portion that dynamically connects the main body part and the 
effector part and-includes a sensor capable of actually measuring 
all forces and torques applied between the main body part and the 
effector part, whereby it is possible to actually measure all 
thrusts from the effector part acting on the main body part. 
[0039] The term "virtual thrust transfer gate" is meant to indicate 
a thrust transfer gate, which is virtually provided in the floating 
mobile object and therefore is physically not present therein, 
and in an existing floating mobile object, which is arithmetically 
divided, for convenience's sake, into a main body part and an 
effector part, it is adapted to be able to estimate, based on a 
measured acceleration value for the main body part, forces and 
torques applied between the main body part and the effector part, 
i.e., all thrusts from the effector part acting on the main body 
part . 

Note that when only the term "thrust transfer gate" is used 
herein, the "virtual thrust transfer gate" is also included unless 
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otherwise specified, 

[0040] The term "thrust estimated value at the virtual thrust 
transfer gate" is meant to indicate a virtual value obtained by 
calculation based on a value measured at the virtual thrust transfer 
5 gate by an acceleration sensor (a main body part acceleration 
measured value) , rather than a value actually measured at a 
physically existing thrust transfer gate. 

[0041] The term "thrust planning part" is meant to indicate a part 
or function of a computer that issues a command to the thrust control 

10 part after calculating a desired thrust that should be applied 
at the thrust transfer gate from the effector part to the main 
body part, in order to realize a target trajectory. 
[0042] The term "thrust control part" is meant to indicate a part 
or function of the computer that outputs a thrust command to an 

15 effector, so as to realize a desired target thrust at the thrust 
transfer gate, by feeding baclc a measured or estimated thrust value 
at the thrust transfer gate. 

[0043] The term "target thrust" is meant to indicate a thrust that 
should be applied at the thrust transfer gate from the effector 
20 part to the main body part, in order to realize a target trajectory. 
[0044] The "target acceleration" is meant to indicate an 
acceleration obtained by differential calculation based on a 
desired target trajectory or a target velocity. 
[0045] The term "thrust force transfer gate system (TTGS) " is meant 
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to indicate a control system, which includes the thrust planning 
part and the thrust control part, and controls a floating mobile 
object provided with a thrust force transfer gate. 
[0046] The term "effector thrust force characteristic" is meant 
5 to indicate the relationship between an effector thrust force 
command, which is inputted to an effector such as a thruster, and 
an effector thrust force, which is actually outputted. Note that 
the effector such as a thruster is generally generates a thrust 
force by means of a stream, the effector thrust force characteristic 
10 includes complicated dynamics affected by hydrodynamic 
resistance/lift, in addition tomechanical dynamics of the effector 
itself . 



EFFECT OF THE INVENTION 

15 [0047] Conventionally, a thrust of a thruster is controlled by 
feeding back a sensor information concerning the position/ velocity 
of a floating mobile object, while the present invention directly 
feeds back a generated thrust to control a thrust of an effector, 
thereby realizing a high-speed response according to a principle. 

20 Thus, the above-mentioned sensing delay problem is alleviated. 
Particularly, when the thrust force transfer gate is provided as 
hardware (see, for example, the first and second instances of the 
first embodiment) , the influence of sensing delay is not increased 
even if the mass of the floating mobile object is large or the 
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thrust of the effector is low. 

[0048] In a floating mobile object control system of the present 
invention, the thrust of the effector is controlled by a thrust 
control part feeding back a thrust at the thrust transfer gate 
5 as described below . This realizes the above-mentioned high-speed 
response. At the same time, by applying to the thrust control 
part an existing control technique (e.g., sliding mode control), 
which offers robustness to nonlinearities, it is possible to 
suppress a nonlinearity of an effector dynamic characteristic, 

10 thereby preventing adverse influence on control performance. 
Conventionally, linearization of the effector dynamic 
characteristic is performed by feedforward compensation based on 
a dynamic characteristic model of the effector, however, it is 
significantly time-consuming and imposes a heavy computation load . 

15 The floating mobile object control system of the present invention 
realizes the linearization by thrust feedback, which does not 
necessarily use the dynamic characteristic model, and therefore 
it is possible to construct a control system with less computation 
load. 

20 [0049] In addition, in the conventional art, an unignorable delay 
in time occurs during a period of time since sensing is carried 
out as described above until a thrust command is actually fed to 
the thruster, and as a result, in order to keep the floating mobile 
object on a predetermined trajectory, it is inevitable, in most 
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cases, to drive the floating mobile object more than necessary, 
for example, because it is required to repeatedly correct an 
overshoot and an undershoot a considerable number of times. The 
task of suitably performing gain tuning of the control system is 
5 extremely significant and requires experience in addition to labor 
and time. 

On the other hand, according to the present invention, it 
is possible to feed a thrust that is actually required by the thruster 
in a timely manner, and therefore it is possible to avoid driving 
10 the thruster and the floating mobile object more than necessary. 
In addition, the floating mobile object is not caused to consume 
thruster drive energy more than necessary, whereby it is possible 
to achieve an effect of reducing energy consumption of the floating 
mobile object. 

15 [0050] Also, in the floating mobile object control system of the 
present invention, influence of unknown disturbances can be 
immediately sensed as thrust error at the thrust transfer gate, 
and compensated for by local feedback control of the thrust control 
part, and therefore levels above the thrust planning part are not 

20 required to take the disturbances into consideration. 
Specifically, by only requiring the thrust planning part to issue 
a command such as "stand still" or "go straight", it is possible 
to automatically compensate for the influence of the disturbances 
in the thrust control part, thereby realizing a desired operation 
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that is previously planned. Also, it is possible to realize a 
high-speed response compared to a conventional case where the 
influence of the disturbances is sensed by position/velocity error . 
[0051] On the other hand, according to a floating mobile object 
5 acceleration sensing system of the present invention, an estimated 
acceleration value for the main body part can be obtained by using 
a previously identified inertia matrix of the main body part, an 
output from the force/torque sensor from the thrust transfer gate, 
and, if necessary, an output from the inclination angle sensor 

10 of the main body part, and therefore it is possible to sense an 
acceleration of the floating mobile object with high precision 
to such an extent that a minor change in acceleration can be sensed, 
regardless of size and weight of the floating mobile object or 
without depending on the capability of the acceleration sensor, 

15 and also possible to, when guiding the floating mobile object by 
inertial navigation, accurately guide the floating mobile object 
onto a desired trajectory. 

Hereinafter, the present invention will be described in 
more detail. 

20 

BRIEF DESCRIPTION OF THE DRAWINGS 

[0052] [FIG. 1] FIG. 1 is a schematic diagram illustrating an 
exemplary configuration of the floating mobile object to which 
a control system of the present invention is applied. 
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[FIG. 2] FIG. 2 is a block diagram illustrating an exemplary 
configuration of the control system of the present invention. 

[FIG. 3] FIG. 3 is a schematic diagram illustrating an 
another exemplary configuration of the floating mobile object to 
5 which the control system of the present invention is applied. 

[FIG. 4] FIG. 4 is a block diagram illustrating an another 
exemplary configuration of the control system of the present 
invention. 

[FIG. 5] FIG. 5 is a schematic diagram illustrating an 
10 another exemplary configuration of the floating mobile object to 
which the control system of the present invention is applied. 

[FIG. 6] FIG. 6 is a block diagram illustrating an another 
exemplary configuration of the control system of the present 
invention. 

15 [FIG. 7] FIG. 7 is a block diagram illustrating a second 

embodiment of the control system of the present invention. 

[FIG, 8] FIG. 8 is a block diagram illustrating an example 
of the control system of the present invention. 

[FIG. 9] FIG. 9 is a block diagram illustrating an another 
20 example of the control system of the present invention. 

[FIG. 10] FIG. 10 is ablockdiagramillustratingananother 
example of the control system of the present invention. 

[FIG. 11] FIG. 11 is a diagram schematically illustrating 
a prior known floating mobile object. 
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[FIG. 12] FIG. 12 is a block diagram schematically 
illustrating a prior known floating mobile object control system. 

[FIG. 13] FIG. 13 is an another block diagram schematically 
illustrating a prior known floating mobile object control system. 

[FIG. 14] FIG. 14 is an another block diagram schematically 
illustrating a prior known floating mobile object control system. 

DESCRIPTION OF THE REFERENCE CHARACTERS 
[0053] A robot arm part 

B main body part 

Bl statics of main body part 

B2 inertia of main body part 

B2 ' inertia of main body part 

B3 inertia of main body part 

B4 integration 

C cable 

E effector part 

El effector thrust characteristic 

E2 statics of effector part 

G thrust transfer gate 

G' virtual thrust transfer gate 

H operator 

P computer 

PI differential calculus 
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P2 thrust planning part 
P3 thrust control part 
P4 position control/velocity control 
S pressure hull 
5 T thruster 

1 control system 

2 acceleration sensor 

3 inclination angle sensor 

4 position/velocity sensor 
10 5 cavity 

10 floating mobile object 

11 subtraction circuit 

12 thruster control circuit 

13 sensor 

15 14 effector thrust characteristic 

15 floating mobile object dynamics 

16 reaction force/torque of robot arm 

17 adding circuit 

18 thruster/robot arm control circuit 
20 19 joint actuator 

20 floating mobile object main body/robot arm complex 
system dynamics 

21 subtraction circuit 
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EMBODIMENTS FOR CARRYING OUT THE INVENTION 
[0054] [First Embodiment] 

Hereinafter, an embodiment of tine present invention and 
the action thereof will be described with reference to FIGS 1 
5 through 7 in relation to an underwater robot, which typifies a 
target for which the present invention is carried out. The 
underwater robot is nothing less than a floating mobile object 
for floating in three-dimensional space, and in the case of applying 
a control system of the present invention to the underwater robot, 

10 it is necessary to compensate for influence of a gravitational 
acceleration, therefore it is required to provide inclination angle 
measurement means capable of measuring an inclination of a main 
body part as defined by the invention according to (7) above. The 
same can be said of the invention according to (5) above to which 

15 the notion of a virtual thrust transfer gate system is applied, 
and in the case of applying a control system of the present invention, 
which employs the notion of the virtual thrust transfer gate system, 
to a floating mobile object for floating in three-dimensional space 
or the lilce , it is required to provide inclination angle measurement 

20 means capable of measuring an inclination of the floating mobile 
object as defined by the invention according to (7) above. 

The inventions according to (1) and (3) or (2) and (4) above 
further including inclination angle measurement means, which is 
the invention according to (7) above, correspond to first and second 
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cases of a first embodiment, respectively, and they also correspond 
to the below-described examples 1 and 2, respectively. The 
inventions according to (5) and (6) above correspond to a third 
case of the first embodiment and the below-described example 3. 

It is fully understood from the above definition section 
and the below-described examples that the application target is 
not limited to the underwater robot. In addition, the underwater 
robot is not limited to a typical example used for deep sea 
exploration as illustrated in the drawings, and relatively small 
robots, which can be used for lifesaving or oil field exploration, 
fall within the category of the underwater robot. 

Here, FIG. 1 is a schematicdiagramillustrating an exemplary 
configuration of the floating mobile object to which a control 
system of the present invention is applied, and FIG. 2 is a block 
diagram illustrating an exemplary configuration of the control 
system of the present invention. FIGS. 3 and 5 are schematic 
diagrams each illustrating an another exemplary configuration of 
the floating mobile object to which the control system of the present 
invention is applied, and FIGS. 4 and 6 are block diagrams each 
illustrating an another exemplary configuration of the control 
system of the present invention. FIG. 7 is a block diagram 
illustrating a second embodiment of the control system of the 
present invention . 

In FIGS. 1 through 7 above, the same elements as those 
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illustrated in FIGS . 11 through 14 illustrated above are described 
with the same reference characters. 

In FIGS. 2, 4, 6 and 1 , a thin solid line extending between 
blocks represents a signal. In contrast, a double line extending 
5 between blocks within a floating mobile object 10 represents a 
phenomenon (dynamic transformation action or the like) taking place 
as a physical action in the floating mobile object 10. 
[0055] Incidentally, in the following detailed description of the 
present invention, a reference coordinate system fixed in the 

10 absolute location of an inertial system is denoted by Zr, and a 
main body coordinate system having the origin at the gravitational 
center position of the floating mobile object and fixed on the 
main body part of the floating mobile object is denoted by Ib- 
Unless otherwise specified, vectors and matrices are indicated 

15 by expressions as they are viewed from the main body coordinate 
system Zb- Further, the floating mobile object is present in 
m-dimensional space (m< 6) , and an effector of the floating mobile 
object is capable of generating an n-degree-of-f reedom thrust (m 

< n) . 

20 [0056] First Instance 

The configuration of a first instance of a thrust transfer 
gate system according to the present embodiment is as shown in 
FIGS. 1 and 2. FIG. 1 is a schematic diagram of a floating mobile 
object, and FIG. 2 is a block diagram of a control system. 

35 



SUBSTITUTE SPECIFICATION 



F-9163 



Inventor: Katsuya KANAOKA 



In the present instance, a floating mobile object 10 is 
dynamically divided into a main body part B and an effector part 
E, and a thrust transfer gate G is provided as hardware between 
the main body part B and the effector part E. In addition, the 
main body part B is provided with an acceleration sensor 2 and 
an inclination angle sensor 3, and the thrust transfer gate G is 
provided with a force/torque sensor. In the present instance, 
the main body part B and the thrust transfer gate G are configured 
to be exposed to exterior similar to the effector part E. 

Note that the below-described inertiamatrix ofthemain 
body part B and J^^ and J^^ representing geometric constructions 
of the floating mobile object 10 are given as previously measured 
known matrices. 

[0057] Hereinbelow, tho configuration of the present instance is 
described in detail. 

Referring to FIG. 2 for the interiors of the main body part 
B and the effector part E, an equation of motion representing 
physical dynamics of the floating mobile object can generally be 
written as the following equation. 
[0058] [Expression 1] 



Equation (1) represents the main body part, and Equation (2) 
represents the effector part . Each variable is defined as follows : 



"g(0 = ^5/(0«^(0 + /e(0 



(1) 



(2) 
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QgiOeR'" : an acceleration of the main body part 

(gravitational center) of the floating mobile object; 

gg(t) eR'" : a gravitational acceleration applied to the 

main body part (gravitational center) of the floating mobile 
5 object; 

Ug{t)sR'^ : a thrust from the effector part acting on 

the main body part at the thrust transfer gate; 

y^yg E : a Jacobian matrix for transformation from 
the velocity of the main body part to the velocity of the thrust 
10 transfer gate; 

^^^^mxm . inertia matrix of the main body part of 

the floating mobile object; 

Af^e : an additional inertia matrix for the main 
body part of the floating mobile object due to influence of a stream; 
15 faiO^R'" ' ^ thrust (an external force such as tidal 

current, waves or wind) acting on the main body part of the floating 
mobile object, which cannot be expressed by other terms in Equation 
(1); 

u^iOeR" : a thrust generated by each effector; 

20 y^(j(/)e /J"""" : a Jacobian matrix for transformation from 

the velocity of the thrust transfer gate to the velocity of each 
effector; 

: a thrust (e.g., an inertia force of an 
effector part, or external force such as tidal current, waves or 
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wind) from the effector part of the floating mobile object acting 
on the thrust transfer gate, which cannot be expressed by other 
terms in Equation (2), and is indicated by an expression as it 
is viewed from the thrust transfer gate. 
5 [0059] Note that the thrust transfer gate system 1 of the present 
instance is structured such that the thrust transfer gate G is 
fixed on the single rigid body structure of the main body part 
B. Thus, the Jacobian matrix J^^ and the inertia matrix are 
constant matrices. 
10 [0060] Described next is a thrust planning part of the present 
instance . 

A target thrust u^^it) at the thrust transfer gate G required 
for realizing a target acceleration ag^{t) of the main body part 
B as indicated by the bloclc of a thrust planning part P2 within 
15 a computer P in FIG. 2 is defined by the following equation. 
[Expression 2] 

[0061] Note that <3g^(0 and a^it) [an m-dimensional vector] are 
respectively a target value and an estimated value for a main body 
20 acceleration Qgit) , and u^jit) [an m-dimensional vector] is a target 
value for a gate thrust u^it) , and w^^O [an m-dimensional vector] 
is a value of u^it) measured by a force/torque sensor provided at 
the thrust transfer gate. 

[0062] Here, by controlling u^Q) with a thrust control part denoted 
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by reference character P3 within the computer P in FIG. 2, such 
that Wg^(0 is realized by thrust feedback, it is possible to 
construct a feedback control system for controlling a thrust by 
thrust feedback with less delay. 

A control technique offering superior robustness to 
nonlinear systems should be applied to the thrust control part 
P3 in order to suppress an unknown nonlinearity of an effector 
thrust characteristic denoted by reference character El within 
an effector part E in FIG. 2 and a dynamic characteristic of the 
effector part such as disturbances or dynamic force f^{t) against 
the effector part . For example , by applying existing sliding mode 
control, the thrust control part P3 can be constructed as below. 
[0063] In the thrust control part P3 of the present instance, a 
thrust command u^^{t) to the effector required for realizing the 
target thrust u^jit) , is defined as in the following equation. 
[0064] [Expression 3] 




(4) 



[w£^/max(k(O]/<0) 



(5) 



Each variable is defined as follows: 



: a switch function for sliding mode control; 



: a time constant of a first-order lag system 
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constrained by switch function; 



a command thrust to each effector; 



u 



EdimtK 



eR 



a maximum commandable thrust to an i-th 



component of the effector thrust; 



a minimum commandable thrust to the i-th 



component of the effector thrust; 




an i-th component of a vector * 



: transposition of a pseudo inverse matrix 



of a matrix *. 

[0065] In the present instance, values required for feedback to 
the thrust planning part and the thrust control part are u^^t) 
and a^t) . As the gate thrust measured value 1/^^(0 / a value measured 
by the force/torque sensor provided at the thrust transfer gate 
is used as it is. 

[0066] Described below is a method for estimating the acceleration 
aQ{t) of the main body part. To examine a sensor characteristic 
and a thrust control characteristic, the following discussion is 
given mainly in relation to a frequency domain. 
[0067] First, a measured acceleration value ^b,(0 [an 
m-dimensional vector] obtained by the acceleration sensor of the 
main body part denoted by reference character 2 within the main 
body part B in FIG . 2 is given as below . The measured value includes 
a gravitational acceleration, and therefore cannot be used as an 
estimated value for the main body acceleration a^if) without any 



SUBSTITUTE SPECIFICATION 
F-9163 Inventor: Katsuya KANAOKA 

change . 

[0068] [Expression 4] 

Oasis) ^G,Mia,{s)-g,{s)) (6) 
[0069] Note tliat *{s) is Laplace transform of the vector or matrix 
5 * = L[*{t)]) , and is a transfer function matrix with m 

rows and m columns representing an acceleration sensor 
characteristic of the main body part. 

Similarly, a measured value g^^CO [an m-dimensional 
vector] for a gravitational acceleration obtained by the 
10 inclination angle sensor of the main body part is given as below. 
[0070] [Expression 5] 

gBsis) = G,ssis)gs(s) (7) 
[0071] Note that G^b^^s) is a transfer function matrix with m rows 
and m columns representing an inclination angle sensor 
15 characteristic of the main body part. 

Similarly, a gate thrust measured value "^^(0 l^^ 
m-dimensional vector] obtained by the thrust transfer gate is given 
as below. 

[0072] [Expression 6] 
20 u^^is) = G^,{s)u^is) (8) 

[0073] Note that G^^(s) is a transfer function matrix with m rows 
and m columns representing a force/torque sensor characteristic 
of the thrust transfer gate. 

[0074] Described next is estimation of the main body acceleration 
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Ogit) in the present instance. 

An estimated value otgit) for the main body acceleration ag{t) 
is defined as below. 
[0075] [Expression 7] 
5 a,(t)^a,^{t)^g^{t) (9) 

[0076] Here, consider the validity of the above estimation of the 
main body acceleration. By applying the Laplace transform to 
equation (9) and substituting equations (6) and (7), 

a,(5) = G,,,(5)(a,(5)-g,(5)) + G,,,Wg,(5) (9) ' 
10 is given, and if the condition "all types of sensor characteristics 
are equal at all degrees of freedom and there is no interference 
between sensors and between the degrees of freedom (a sensor 
characteristic consistent condition) " is satisfied, the 
acceleration sensor characteristic of the main body part, the 
15 inclination angle sensor characteristic of the main body part and 
the force/torque sensor characteristic of the thrust transfer gate 
may be defined as follows: 

GaBsis) = = G^,is) = GMI (10 ) , 

where G^(s) is a transfer function representing an equal sensor 
20 characteristic, and I is a unit matrix. That is, 

aBs(s) = G,{s)ia,(s)-g,is) (11), 

gM^G^(s)g,{s) (12), and 

Uas(s) = GMt^c(^) (13) . 

In this case, equation (9) ' is given as below. 
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[0077] [Expression 8] 

as{s) = G,is)a,{s) (14) 
[0078] Thus, the estimation of the main body part acceleration 
agio can be performed by equation (8) , A gravitational 
5 acceleration gfi,(0 does not explicitly appear in equation (14), 
and therefore it is possible to consider that the main body part 
acceleration Og{i) is directly measured so long as the above sensor 
characteristic consistent condition is satisfied. 

Note that the above sensor characteristic consistent 
10 condition is realized by, for example, selecting a sufficiently 
high-speed response and high-precision sensor compared to the 
dynamics of the floating mobile object. 
[0079] Second Instance 

Described next is a second instance of the thrust transfer 
15 gate system according to the present embodiment. 

The second instance of the thrust transfer gate system is 
configured as shown in FIGS . 3 and 4 . FIG . 3 is a schematic diagram 
of a floating mobile object, and FIG. 4 is a block diagram of a 
control system. 

20 In the present instance also, a floating mobile object 10 

is dynamically divided into a main body part B and an effector 
part E, and a thrust transfer gate G is provided as hardware between 
the main body part B and the effector part E. In addition, the 
main body part B is provided with an inclination angle sensor 3, 

43 



SUBSTITUTE SPECIFICATION 
F-9163 Inventor: Katsuya KANAOKA 

and the thrust transfer gate G is provided with a force/torque 
sensor. 

The present instance significantly differs from the first 
instance in that the effector part E is a shell for covering the 
5 main body part B (a pressure hull S shown in FIG. 3) , and therefore 
the main body part B and the thrust transfer gate G are completely 
isolated from the exterior by the shell S. This structure 
eliminates the necessity of the acceleration sensor 2 of the main 
body part B, which is required in the first instance. 
10 As in the first instance, an inertia matrix of the main 

body part and J^g and J^q representing geometric constructions of 
the floating mobile object are given as previously measured known 
matrices . 

[0080] Hereinbelow, the configuration of the present instance is 
15 described in detail. 

Referring to FIG. 4 for the interiors of the main body part 
B and the effector part E, an equation of motion representing 
physical dynamics of the floating mobile object can generally be 
written as the following equation. 
20 [0081] [Expression 9] 

^/"G(0 = ^^K(0-g/,(0) (15) 

UciO^JEo'^iOUsiO'^fEiO (16) 
[0082] Equation (15) represents the main body part, and equation 
(16) represents the effector part . Here, u^jiO [an m-dimensional 
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vector] is defined anew as in the following equation. 
[0083] [Expression 10] 

UciiO = Uait)'^JGB'''MB8BiO (17 ) 

[0084] Described next is a thrust planning part in the present 
5 instance. 

A target thrust u^^^it) at the thrust transfer gate G required 
for realizing a target acceleration a^^it) of the main body part 
B as indicated by the block of the thrust planning part P2 within 
a computer P in FIG. 4 is defined by the following equation. 

10 [0085] [Expression 11] 

^G2,if)^JGB~''M,aM (18) 
[0086] By controlling ^^2(0 with a thrust control part denoted by 
reference character P3 within the computer P in FIG. A, such that 
"c2rf(0 is realized by thrust feedback, it is possible to construct 

15 a feedback control system for controlling a thrust by thrust 
feedback with less delay. 

Here, forexample, by applying slidingmode control to thrust 
control in the present instance, it is possible to construct the 
thrust control part P3 as below. 

20 [0087] In the thrust control part P3 of the present instance, a 
thrust command u^^{t) to the effector required for realizing a 
target thrust Wc2t/(0 is defined as in the following equation. 
[0088] [Expression 12] 
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Sci0-JEG''i0\iUo2(0'Ua,A0)^^^ (19) 



k./(0]/= 



^'E./max(k(O],<0) 

0 (k(0].= 0) (20) 



[0089] Note that i/^zCO [^^i m-dimensional vector] is an estimated 
value for w^zCO • 

5 [0090] Incidentally, in the present instance, no feedback to the 
thrust planning part P2 is required, and only the estimated value 
u^jit) for Wc2(0 equation (17) is required for feedback to the 
thrust control part P3. Here, a method for estimating m^zW is 
described. 

10 [0091] Hereinbelow, estimation of the gate thrust Mg2(0 i^i the 
present instance is described. 

The estimated value u^^iO ^or the gate thrust w^zCO is 
defined as in the following equation. 
[0092] [Expression 13] 

1 5 Wg2(0 = Wg, (0 + Jci'j^BgssiO (21) 

[0093] Here, consider the validity of the estimation of the gate 

thrust. By applying the Laplace transform to equation (21), 

UG2i^) = u^si^)-^Jas''M,gM (21) ' 

is given, and if the aforementioned sensor characteristic 
20 consistent condition is also satisfied here, equations (12) and 
(13) are established similarly. By substituting these and 
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considering equation (17), the following is given. 

[0094] [Expression 14] 

UG2(^) = G,is)u^,is) (22) 

[0095] Tlius, the gate thrust w^zW can be estimated by equation 
5 (21) . A gravitational acceleration ^^,(0 does not explicitly 

appear in equation (22) , and therefore it is possible to consider 

that the gate thrust u^^iO is directly measured so long as the 

sensor characteristic consistent condition is satisfied. 

[0096] Third Instance 
10 Described next is a third instance of the thrust transfer 

gate system according to the present embodiment. 

The third instance of the thrust transfer gate system is 

configured as shown in FIGS . 5 and 6 . FIG . 5 is a schematic diagram 

of a floating mobile object, and FIG. 6 is a bloc]c diagram of a 
15 control system. 

In the third instance, the floating mobile object 10 is 

not dynamically, divided into a main body part B and an effector 

part E, and no thrust transfer gate G is provided as hardware. 

However, the floating mobile object 10 of the present instance 
20 is provided with an acceleration sensor 2 and an inclination angle 

sensor 3. 

In the present instance, a thrust transfer gate system is 
constructed as if a thrust transfer gate is present by suitably 
determining an inertia matrix Mg of a main body part B and 
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calculating, based on Mg, an estimated thrust value at a virtual 
thrust transfer gate G' shown within a computer P in FIG. 6 from 
a value measured by the sensor. As in the above first and second 
instances, and J^.^ representing geometric constructions of 

the floating mobile object 10 are given as previously measured 
known matrices. 

Note that examples of a method for obtaining the inertia 
matrix Mq of the main body part B include a method in which the 
computer previously calculates the inertia matrix Mq of the main 
body part B based on drawings, specifications, etc., of the floating 
mobile object 10 as originally designed and stores it in an 
unillustrated memory or the like. 

[0097] Hereinbelow, the configuration of the present instance is 
described in detail. 

Referring to FIG. 6 for the interiors of the main body part 
B and the effector part E, an equation of motion representing 
physical dynamics of the floating mobile object can generally be 
written as the following equation. 
[0098] [Expression 15] 

JoB'uaM) = M,a,{t) (23) 

«Gv(0 = V(/K(0 + /.(0 + -/G/Xg.(0 (24 ) 

[0099] Equation (23) represents the main body part, and equation 
(24 ) represents the effector part . This is approximately the same 
as in the second instance, but it should be noted that there is 
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no physical thrust transfer gate in the third instance, and 
therefore a thrust u^^ [an m-dimensional vector] at the thrust 
transfer gate is a virtual value, 

[0100] Described next is a thrust planning part of the present 
instance . 

A target thrust u^y^it) at a virtual thrust transfer gate 
G' required for realizing a target acceleration a^^(0 of the main 
body part B as indicated by the blocic of a thrust planning part 
P2 within a computer P in FIG. 6 is defined as in the following 
equation. 

[0101] [Expression 16] 

^^g.AO = Jgb''MbC^bAO (25) 
[0102] By controlling Wcv(0 with a thrust control part denoted by 
reference character P3 within the computer P in FIG. 6, such that 
^GvdiO is realized by thrust feedbaclc, it is possible to construct 
a feedbaclc control system for controlling a thrust by thrust 
feedbaclc with less delay. 

Here, forexample, by applying slidingmode control to thrust 
control in the present instance, it is possible to construct the 
thrust control part P3 as below. 

[0103] In the thrust control part P3 of the present instance, a 
thrust command u^^it) to the effector required for realizing the 
target thrust u^^it) is defined as in the following equation. 

[0104] [Expression 17] 
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(26) 



k^/max(l'yc(O]/<0) 

k.(0].= 0 (k(0].= o) 



(27) 



[0105] Note that Uq^O [an m-dimensional vector] is an estimated 

value for w^^CO . 

[0106] Incidentally, in tlie present instance, no feedbaclc to the 
thrust planning part P2 is required as in the second instance, 
and only the estimated value m^vW fo^ ^gvW is required for 
feedback to the thrust control part P3. Here, a method for 
estimating w^vCO is described. 

[0107] Hereinbelow, estimation of the gate thrust m^^CO in the 
present instance is described. 

The estimated value u^XO for the gate thrust is 
defined as in the following equation. 
[0108] [Expression 18] 



[0109] Here, consider the validity of the estimation of the gate 
thrust. By applying the Laplace transform to equation (28), 



is given, and if the aforementioned sensor characteristic 
consistent condition is also satisfied here, equations (11) and 
(12) are established similarly. By substituting these and 



(28) 



«cv (s) = Jgb'^^ b W 8bs i^)) 



(28) ' 
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considering equation (23)/ the following is given. 
[0110] [Expression 19] 

Uc.is)^G,is)UaM (29) 
[0111] Thus, the gate thrust w^vCO can be estimated by equation 
5 (28) . A gravitational acceleration g^iCO does not explicitly 
appear in equation (29) , and therefore it is possible to consider 
that the gate thrust u^^O) is directly measured so long as the above 
sensor characteristic consistent condition is satisfied. 
[0112] [Second Embodiment] 

10 Described next is an embodiment of a floating mobile object 

acceleration sensing system of the present invention. As in the 
first embodiment, the present embodiment is described in relation 
to an underwater robot as an application target. Note that the 
underwater robot is nothing less than a floating mobile object 

15 for floating in three-dimensional space, and in the case of applying 
an acceleration sensing system of the present invention to the 
underwater robot, it is necessary to compensate for influence of 
a gravitational acceleration as in the first embodiment, therefore 
it is required to provide inclination angle measurement means 

20 capable of measuring an inclination of a main body part. 

In the present embodiment, a thrust transfer gate is used 
for estimating an acceleration of the main body part. The 
acceleration sensing system according to the present embodiment 
is configured as shown in FIGS. 3 and 7. FIG. 3 is a schematic 
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diagram of a floating mobile object, and FIG. 7 is a block diagram 
of the acceleration sensing system. 

[0113] Ineachinstanceof the first embodiment, the thrust transfer 
gate system is constructed as the control system 1 for the floating 
5 mobile object 10, whereas in the second embodiment, a thrust 
transfer gate G is used as a high-precision acceleration sensing 
system for the floating mobile object 10. 

[0114] Hardware configuration of the present embodiment is exactly 
the same as that of the second instance (FIG. 3) according to the 

10 above first embodiment . Specifically, the floating mobile object 
10 is dynamically divided into a main body part B and an effector 
part E, the main body part B is provided with an inclination angle 
sensor 3, and the thrust transfer gate G is provided with a 
force/torque sensor. 

15 However, the present embodiment aims to estimate an 

acceleration a^it) of the main body part with high precision, rather 
than control it, and therefore the present instance does not include 
the thrust planning part and the thrust control part. 

In addition, an inertia matrix Af^ of the main body part 

20 B and J^^q representing a geometric construction of the floating 
mobile object are given as previously measured known constant 
matrices. By suitably designing the inertia matrix Mg of the main 
body part with consideration of the measuring range of the 
force/torque sensor at the thrust transfer gate and an acceleration 
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range that should be sensed in the present embodiment, it is made 
possible to measure an acceleration with high precision. 
[0115] Hereinbelow, the configuration of the present embodiment 
is described in detail. 
5 The acceleration OgQ) of the main body part in the hardware 

configuration of the present embodiment as shown* in FIGS. 3 and 
7 is represented by transforming equation (15) as below. 
[0116] [Expression 20] 

a,(0 = A//^/i/c(0 + g,(0 (30) 
10 [0117] Described next is estimation of the acceleration a^Q) of 
the main body part in the present embodiment. 

An estimated value a^(0 for the acceleration a^it) of the 
main body part is defined as in the following equation. 
[0118] [Expression 21] 

15 a,it) = Mi'Jas'uasiO + gssiO ( 31 ) 

[0119] Here, consider the validity of the estimation of the main 
body acceleration- By applying the Laplace transform to equation 
(31), the following is given. 
[0120] [Expression 22] 
2 0 a, (s) = Mi'jjua, is) + g,, {s) ( 32 ) 

[0121] If the aforementioned sensor characteristic consistent 
condition is also satisfied here, equations (12) and (13) are 
established as in the first embodiment. By substituting these 
and considering equation (30), the following is given. 
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[0122] [Expression 23] 

asis)^G,(s)a,is) (33) 
[0123] Thus, it is appreciated that the main body part acceleration 
Qgit) can be estimated by equation (31) . 
5 [0124] Example 1 

As described in the above first embodiment section, the 
floating mobile object control system of the present invention 
performs thrust control, and therefore, to be strict, an 
acceleration trajectory, rather than a position/velocity 

10 trajectory, is realized. Here, it is not possible to avoid 
influence of drift caused by integration error in acceleration, 
and therefore the motion on which the present invention solely 
exhibits a remarkable effect is that in a relatively high frequency 
range. As for low-frequency slow motion, it is necessary to 

15 additionally use a conventional control technique for feeding back 
a position/velocity, but in such a case also, it is possible to 
improve precision compared to the case of using the conventional 
technique alone. 

Hereinbelow, an example of the present invention is 

20 described in relation to a control system employing conventional 
position/velocity feedback control in addition to each instance 
of the floating mobile object control system of the present 
invention as described in the above first embodiment. 
[0125] Here, FIG. 8 is a block diagram illustrating an example 
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of the control system of the present invention corresponding to 
the first instance of the first embodiment, FIG. 9 is a block diagram 
illustrating an another example of the control system of the present 
invention corresponding to the second instance of the first 
5 embodiment, and FIG. 10 is a block diagram illustrating an another 
example of the control system of the present invention 
corresponding to the third instance of the first embodiment. In 
FIGS. 8 through 10, the same elements as those shown in FIGS. 1 
through 7 and 11 through 14 above are described with the same 

10 reference characters. 

In addition, thin solid lines extending between blocks in 
FIGS 8 through 10 represent signals as in FIGS. 2, 4, 6 and 7. 
On the other hand, double lines extending between blocks in a 
floating mobile object 10 represent phenomena (dynamic 

15 transformation action or the like) taking place as a physical action 
in the floating mobile object 10. 

[0126] Example 1 corresponds to the above first instance of the 
first embodiment . FIG . 8 illustrates a block diagram of a control 
system 1 according to the present example. A schematic 
20 configuration of the floating mobile object 10 of the present 
example is as shown in FIG. 1. 

As is apparent from the block diagram in FIG. 8, in the 
present example, the configuration shown in FIG. 2 and described 
in the first instance of the first embodiment is applied to a prior 
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known position/velocity feedback control system. Accordingly, 
the configuration of a thrust transfer gate system portion of the 
present invention, the operation of the control system, etc., are 
described with reference to FIGS. 1 and 2 above. 
5 [0127] The control system 1, i.e., the thrust transfer gate system 
according to the present example, is mainly characterized in that 
the mechanism of the floating mobile object denoted by reference 
character 10 is dynamically divided into a main body part B, which 
receives a thrust, and an effector part E, which generates a thrust, 

10 and a thrust transfer gate G is added to a connection therebetween. 
Here, the thrust transfer gate G is an only portion that connects 
the main body part B with the effector part E and includes a sensor 
(a force/torque sensor) capable of actually measuring all forces 
and torques applied between the main body part B and the effector 

15 part E, Note that as the force/torque sensor, a commercially 
available general-purpose product (e.g. , a multi-channel product 
capable of outputting multi-dimensional force or torque 
components) can be used. 

As such, in the present example, by connecting the main 

20 body part B and the effector part E with the thrust transfer gate 
G, all thrusts from the effector part E acting on the main body 
part B can be measured. In the floating mobile object 10 of the 
present example to which the thrust transfer gate system is applied, 
the main body part B is provided with an acceleration sensor 2 

56 



SUBSTITUTE SPECIFICATION 
F-9163 Inventor: Katsuya KANAOKA 

and an inclination angle sensor 3 as shown in FIG. 1. 
[0128] The thus-configured control system 1 according to the 
present example performs thrust feedback control by the thrust 
transfer gate system of the present invention simultaneously as 
5 prior known position/velocity feedback control. 

[0129] According to the present example, while additionally using 
a conventional control technique for feeding back a 
position/velocity to avoid influence of drift causedby integration 
error in acceleration, a control system, i.e., a thrust transfer 

10 gate system of the present invention, is used to allow a floating 
mobile object to stand still in a predetermined position with high 
precision, or track a target trajectory with high precision, even 
under disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 

15 to the f loatingmobile object, thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 
[0130] Example 2 

The present example corresponds to the above second instance 
of the first embodiment. FIG. 9 illustrates a block diagram of 

20 a control system 1 according to the present example. A schematic 
configuration of a floating mobile object 10 of the present example 
is as shown in FIG. 3 above. 

As in example 1, the control system 1 according to the present 
example is mainly characterized in that the mechanism of the 
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underwater robot 10 is dynamically divided into a main body part 
B, which receives a thrust, and an effector part E, which generates 
a thrust, and a thrust transfer gate G is added to a connection 
therebetween . 

Note that the present example differs from the 
above-described example 1 in that the main body part B is contained 
in the effector part E, and external disturbances, reaction of 
a robot arm A, etc., all affect the effector part E, which is a 
pressure hull S. Here, the main body part B is supported only 
by the thrust transfer gate G in the effector part E, Therefore, 
as in the first example, the thrust transfer gate G is an only 
portion that connects the main body part B with the effector part 
E. 

Accordingly, in the present example also, by connecting 
the main body part B and che effector part E with the thrust transfer 
gate G, all thrusts from the effector part E acting on the main 
body part B can be actually measured. 

[0131] According to the second example also, while additionally 
using a conventional control technique for feeding back a 
position/velocity to avoid influence of drift caused by integration 
error in acceleration, a control system, i.e., a thrust transfer 
gate system of the present invention, is used to allow a floating 
mobile object to stand still in a predetermined position with high 
precision, or track a target trajectory with high precision, even 
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under disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
to the floating mobile object, therebymaking it possible to control 
the floating mobile object at higher speed with higher precision. 
5 [0132] Example 3 

The present example corresponds to the above third instance 
of the first embodiment. FIG. 10 illustrates a block diagram of 
a control system 1 according to the present example. A schematic 
configuration of a floating mobile object 10 of the present example 

10 is as shown in FIG. 5. 

The control system 1 according to the present example is 
mainly characterized in that the mechanism of the underwater robot 
is, in an arithmetical manner, divided into a main body part B, 
which receives a thrust, and an effector part E, which generates 

15 athrust, andavirtual thrust transfer gate G' is virtually provided 
therebetween. 

Note that the virtual thrust transfer gate G' is able to 
estimate all forces and torques applied between the main body part 
B and the effector part E. 
20 [0133] According to the third example also, while additionally 
using a conventional control technique for feeding back a 
position/velocity to avoid influence of drift caused by integration 
error in acceleration, a control system, i.e., a thrust transfer 
gate system of the present invention, is used to allow a floating 
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mobile object to stand still in a predetermined position with high 
precision, or track a target trajectory with high precision, even 
under disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
5 to the f loatingmobile object, thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 
[0134] [Implementation by utilizing an inertial navigation 
device] 

Incidentally, inertial navigation devices based on 
10 existing technology widely used for floating mobile objects such 
as airplanes can be used as acceleration sensors , inclination angle 
sensors and main body part position/velocity sensors, which are 
used in the present invention. 

Specifically, a floating mobile object having an inertial 
15 navigation device mounted therein can implement a control system 
according to example 3 or the above third instance of the first 
embodiment of the present invention by changing only software 
without adding/changing any hardware component, and therefore it 
is possible to readily realize an increase in speed and precision 
20 of an existing floating mobile object. 

Todescribe itmore specifically, it is the current situation 
that an existing inertia 1 navigation device measures accelerations 
with sufficiently high precision, but only position/velocity 
information obtained by integrating them is used for control. 
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Therefore, by using acceleration information, which has never been 
used as it is, for measuring a thrust within the framework of a 
thrust transfer gate system of the present invention, it is made 
possible to realize an increase in speed and precision compared 
to existing control without changing any conventional hardware 
component . 

Particularly, as for the inertial navigation device, most 
recent inertial navigation devices are of a hybrid type in which 
an acceleration sensor, a gyro, a GPS (Global Positioning System) 
and other sensors are integrated into a unit to obtain acceleration, 
velocity and displacement information, and therefore the measuring 
speed is increased on the whole. Accordingly, the inertial 
navigation device can be considered as a suitable sensor unit that 
can be used for implementing a control system of the present 
invention . 

[0135] Note that the configuration as described in each of the 
above examples is not restrictive, and various modifications in 
design and other variations can be made to the present invention. 

While each of the above examples has been described with 
respect to an underwater robot to which a control system of the 
present invention is applied, the target to which the control system 
of the present invention is applied is not limited to the underwater 
robot. As described in the above definition section, in addition 
to the underwater robot, the control system of the present invention 
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is applicable to floating mobile objects in water, air, outer space, 
etc., such as submersibles , helicopters, airships, aircrafts, 
spaceships or space robots. In addition, the control system of 
the present invention is applicable to air-cushion vehicles for 
5 sliding on water or ground, cars and other vehicles with slipping 
wheels, and various mobile objects for sliding on a plane. 

Further, the instances of the above embodiment and the above 
examples have been described with respect to the case where a control 
system or an acceleration sensing system of the present invention 

10 is applied to a floating mobile object for floating in 
three-dimensional space. However, when applying the control 
system or acceleration sensing system of the present invention 
to a floating mobile object for floating in outer space where 
basically no gravity is applied or a floating mobile object for 

15 sliding on a horizontal plane for which it is not necessary to 
consider any inclination or it is possible to ignore any inclination, 
the inclination angle measurement means as defined by the 
inventions according to (1) through (6) above is not required. 
In this case, by configuring the thrust planning part and the thrust 

20 control part, such that a gravitational acceleration applied to 
the main body part (gravitational center) of a floating mobile 
object is set to ga = 0, it is made possible to define a control 
law in a more simplified manner. 

In addition, in each of the above instances, sliding mode 
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control based on an existing control technique offering robustness 
to a nonlinearity of an effector dynamic characteristic is taken 
as an example of a control law for the thrust control part P3, 
but the control law for the thrust control part P3 is not limited 
5 to this. For example, as the control low for the thrust control 
part P3, it is possible to apply PID control of input power to 
the thrusters T with respect to thrust error between a target thrust 
at the thrust transfer gate G and a measured or estimated thrust 
at the thrust transfer gate G. 

10 Further, in each of the above instances, a thrust generation 

device based on propellers dynamically connected to a motor, which 
is a driving source, is used as a thruster T, but the configuration 
of the thruster T is not limited to this. The thruster T may be 
of a jet type and a driving source thereof is not limited to a 

15 motor. Various types of driving sources such as an internal 
combustion engine, a steam turbine and a nuclear turbine may be 
used. 

[0136] As described in detail above, it is apparent that the present 
invention is a novel and extremely useful invention, which provides 
20 a system structure capable of allowing a floating mobile object 
to stand still in a predetermined position with high precision, 
or track a target trajectory with high precision, even under 
disturbances caused by waves and tidal current or wind or 
disturbances caused by reaction, etc., of a robot arm attached 
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to the floating mobile object, thereby making it possible to control 
the floating mobile object at higher speed with higher precision. 
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